// GPU Verlet Integrator
// Autor: Juan G. Campa

#ifndef _DISTANCECONSTRAINT_CU_
#define _DISTANCECONSTRAINT_CU_


// includes, C string library
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

// includes, cuda
#include <cuda.h>
#include <builtin_types.h>
#include <cuda_runtime_api.h>

#include <Common.cu>
// includes, project
//#include <cutil_inline.h>

#define clampValue(x,y,z) x<y?y:x>z?z:x

// OBSOLETO, usar la version batched.
extern "C" __global__ void DistanceConstraint(float4 *posArray, float4 *springArray, const int springCount, const float dt)
{ 
    unsigned int i = blockDim.x * blockIdx.x + threadIdx.x;

    if (i < springCount)
    {
        float4 spring = springArray[i];
        int index1 = __float_as_int(spring.x);
        int index2 = __float_as_int(spring.y);
        float restLength = spring.z;
        float k = spring.w;

        float4 x1 = posArray[index1];
        float4 x2 = posArray[index2];
        float4 delta = x2 - x1;
        float deltaLength = sqrt(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
        float diff = (deltaLength - restLength)/ deltaLength;
        x1 = x1 + delta * 0.5f * diff;
        x2 = x2 - delta * 0.5f * diff;

        posArray[index1] = x1;
        posArray[index2] = x2;
    }
}

extern "C" __global__ void BatchedDistanceConstraint(
    float4 *posArray,
    float4 **springArrays,
    int *springArraySizes,

    const int springArrayCount,
    const float dt)
{
    unsigned int i = blockDim.x * blockIdx.x + threadIdx.x;

    // Idea: the springArraySizes could be loaded into shared memory first.
    // or maybe each element could be loaded by the first thread only.
    for (int j = 0; j < springArrayCount; j++)
    {
		__syncthreads();
		// Cantidad de resortes en este set.
        int springCount = springArraySizes[j];
        if (i < springCount)
        {
            float4 spring = springArrays[j][i];
            int index1 = __float_as_int(spring.x);
            int index2 = __float_as_int(spring.y);
            float restLength = spring.z;
            float k = spring.w;

            float4 x1 = posArray[index1];
            float4 x2 = posArray[index2];
            float4 delta = x2 - x1;
            float deltaLength = sqrt(delta.x * delta.x + delta.y * delta.y + delta.z * delta.z);
            float diff = (deltaLength - restLength)/ deltaLength;
            x1 = x1 + delta * 0.5f * k * diff;
            x2 = x2 - delta * 0.5f * k * diff;

            posArray[index1] = x1;
            posArray[index2] = x2;
        }
    }
}

extern "C"
void ExecuteDistanceConstraint(dim3 block, dim3 grid, float4* pos, float4* springs, unsigned int springCount, int iterations, float dt)
{
    DistanceConstraint<<<grid, block>>>(pos, springs, springCount, dt);

    // Revisamos si hubo un error.
    cudaError_t error = cudaGetLastError();
    if (error != cudaSuccess) {
        printf("DistanceConstraint() failed to launch error = %d\n", error);
    }
}


  /*float4 *posArray,
    float4 **springArrays,
    int *springArraySizes,

    const int springArrayCount,
    const float dt)*/

extern "C"
void ExecuteBatchedDistanceConstraint(dim3 block, dim3 grid, float4 *pos, float4 **springArrays, int *springArraySizes, unsigned int springArrayCount, int iterations, float dt)
{
    BatchedDistanceConstraint<<<grid, block>>>(pos, springArrays, springArraySizes, springArrayCount, dt);

    // Check for errors
    cudaError_t error = cudaGetLastError();
    if (error != cudaSuccess) {
        printf("DistanceConstraint() failed to launch error = %d\n", error);
    }
}

#endif